function [b, a] = custom_butterLow(n, Wn)
    % Pre-warp cutoff frequency
    fs = 2; % Sampling frequency (normalized)
    Wn_analog = 2 * fs * tan(pi * Wn / fs);
    % Get Butterworth analog low-pass prototype
    [zs, ps, ks] = custom_buttap(n);
    % Convert prototype to state-space form
    [a, b, c, d] = simple_zp2ss(zs, ps, ks);
    % Transform to desired low-pass filter
    [ad, bd, cd, dd] = custom_lp2lp(a, b, c, d, Wn_analog);
    % Convert to digital filter using bilinear transformation
    [ad, bd, cd, dd] = custom_bilinear(ad, bd, cd, dd, fs);
    % Convert to transfer function form
    [z, p, k] = simple_ss2zp(ad, bd, cd, dd);
    [b, a] = custom_zp2tf(z, p, k);
end

function [z, p, k] = custom_buttap(n)
    % 返回零点 (z)、极点 (p) 和增益 (k)
    % 零点为空
    z = [];
    % 极点计算
    angles = (pi * (1:2:n-1) / (2 * n) + pi / 2); % 计算极点角度
    ptemp = exp(1i * angles); % 计算复数极点
    p = [ptemp(:); conj(ptemp(:))]; % 极点对称分布
    if mod(n, 2) == 1 % 若为奇数阶，添加极点 -1
        p = [p; -1];
    end
    % 计算增益
    k = prod(-p); % 增益通过极点计算
end

function [A, B, C, D] = simple_zp2ss(z, p, k)
    % SIMPLE_ZP2SS Converts zero-pole-gain to state-space representation.
    % Input:
    %   z - Zero(s) (can be empty if no zeros exist)
    %   p - Pole(s)
    %   k - Gain
    % Output:
    %   A, B, C, D - State-space representation matrices

    % Step 1: Get the polynomial coefficients from zeros and poles
    n = length(p); % Degree of the denominator
    m = length(z); % Degree of the numerator
    
    % Polynomials
    den = poly(p); % Denominator coefficients
    num = k * poly(z); % Numerator coefficients (scaled by gain)
    
    % Ensure numerator degree matches denominator by appending zeros
    if m < n
        num = [zeros(1, n - m), num];
    end
    
    % Step 2: Construct state-space matrices in controller canonical form
    A = [zeros(n-1, 1), eye(n-1); -den(end:-1:2) / den(1)];
    B = [zeros(n-1, 1); 1];
    C = (num(end:-1:2) - num(1) * den(end:-1:2)) / den(1);
    D = num(1) / den(1);
end

function [A_new, B_new, C_new, D_new] = custom_lp2lp(A, B, C, D, wc)
    % LP2LP: Convert a normalized low-pass filter to a target cutoff frequency.
    % Inputs:
    %   A, B, C, D: State-space matrices of the original filter.
    %   wc: Desired cutoff frequency.
    % Outputs:
    %   A_new, B_new, C_new, D_new: State-space matrices of the transformed filter.
    
    % Apply frequency scaling
    A_new = wc * A;
    B_new = wc * B;
    C_new = C;
    D_new = D;
end

function [A_d, B_d, C_d, D_d] = custom_bilinear(A, B, C, D, fs)
    % 双线性变换函数
    % Inputs:
    %   A, B, C, D: 模拟滤波器的状态空间矩阵
    %   fs: 采样频率
    % Outputs:
    %   A_d, B_d, C_d, D_d: 数字滤波器的状态空间矩阵

    T = 1 / fs; % 采样周期

    % 计算 (I - T/2 * A) 和 (I + T/2 * A)
    I = eye(size(A));
    T_half_A = (T / 2) * A;

    % 数字滤波器矩阵变换
    A_d = (I - T_half_A) \ (I + T_half_A);
    B_d = (I - T_half_A) \ B;
    C_d = C / (I - T_half_A);
    D_d = D + C / (I - T_half_A) * (T / 2) * B;
end

function [z, p, k] = simple_ss2zp(A, B, C, D)
    % SIMPLE_SS2ZP 将状态空间表示转换为零极点增益表示。
    % 输入:
    %   A, B, C, D - 状态空间矩阵
    % 输出:
    %   z - 零点
    %   p - 极点
    %   k - 增益
    
    % Step 1: 计算极点（特征值）
    p = eig(A);
    
    % Step 2: 计算增益 k
    % 增益是通过评估状态空间系统的传递函数得到的
    % H(s) = C(sI - A)^(-1)B + D
    % 增益可以通过计算转移函数的静态增益（s = 0）得到：
    k = C / ( -A ) * B + D; 
    
    % Step 3: 计算零点（多项式的根）
    % 使用系数矩阵来计算零点的值
    num = C * inv(eye(size(A)) - A) * B + D; % 使用矩阵的逆来近似零点
    z = roots(num); % 计算零点
    
end

function [num, den] = custom_zp2tf(z, p, k)
    % ZP2TF 将零点、极点、增益转换为传递函数
    % 输入:
    %   z - 零点向量
    %   p - 极点向量
    %   k - 增益
    % 输出:
    %   num - 分子系数
    %   den - 分母系数
    
    % Step 1: 计算零点的多项式
    num = 1;
    for i = 1:length(z)
        num = conv(num, [1 -z(i)]); % 生成 (s - z_i)
    end
    
    % Step 2: 计算极点的多项式
    den = 1;
    for i = 1:length(p)
        den = conv(den, [1 -p(i)]); % 生成 (s - p_j)
    end
    
    % Step 3: 乘以增益
    num = k * num;
end






